#include <stdio.h>
#include "platform.h"
#include "xparameters.h"
#include "sleep.h"
#include "zrcar.h"
#define MOVE_FORWARD  	1500
#define MOVE_BACK	  	100
#define DANGEROUS_DIS 	100
#define MAX_DIS			1000000
#define MOVE_SLEEP	  	964800
#define TURN_SLEEP	  	400000
#define TURN_SPEED 	    16
float ult_data[3];
int car_x = 0;
int car_y = 0;
int face = 0;
void car_stop()
{
	zrcar_wheel_l_set(0);
	zrcar_wheel_r_set(0);
}
void car_forward()
{
	zrcar_wheel_l_set(-16);
	zrcar_wheel_r_set(-16);
}
void car_back()
{
	zrcar_wheel_l_set(16);
	zrcar_wheel_r_set(16);
}
void car_right()
{
	zrcar_wheel_l_set(0);
	zrcar_wheel_r_set(-TURN_SPEED);
}
void car_turn_r()
{
	zrcar_wheel_r_set(TURN_SPEED);
	zrcar_wheel_l_set(-TURN_SPEED);
}
void car_left()
{
	zrcar_wheel_l_set(-TURN_SPEED);
	zrcar_wheel_r_set(0);
}
void car_turn_l()
{
	zrcar_wheel_l_set(TURN_SPEED);
	zrcar_wheel_r_set(-TURN_SPEED);
}
void make_map(int map[][50], int old_map[][50]){
	int j,k;
	int ite_time = 0;
	for(j=0;j<50;j++){
		for(k=0;k<50;k++){
			map[j][k] = 1;
			old_map[j][k] = 1;
		}
	}
	int sum = 0;
//	int i;
	while(1)
	{
		sum = 0;
		if(ite_time == 100){
			for(j=0;j<50;j++){
				for(k=0;k<50;k++){
					if(old_map[j][k] == map[j][k]){
						sum ++;
					}
					printf("%d",map[j][k]);
					old_map[j][k] = map[j][k];
				}
				printf("\n");
			}
			ite_time = 0;
		}

		ite_time ++;

		zrcar_ultra_get_all(ult_data);
//		for(i = 1; i < 4; i++){
//			printf("u%d : %f mm ",i , ult_data[i-1]);
//		}
//		printf("\n");

		if(ult_data[0] < DANGEROUS_DIS && ult_data[1] < DANGEROUS_DIS
		   && ult_data[2] < DANGEROUS_DIS)
		{
			//left corner && right corner
			//turn left
			face = ( face + 2 ) % 4;
			car_turn_l();
			usleep(2*MOVE_SLEEP);
		}
		else if(ult_data[0] < DANGEROUS_DIS && ult_data[1] < DANGEROUS_DIS)
		{
			//left corner
			//turn right
			face = ( face + 1 ) % 4;
			car_turn_l();
			usleep(MOVE_SLEEP);
		}
		else if(ult_data[1] < DANGEROUS_DIS && ult_data[2] < DANGEROUS_DIS){
			//right corner
			//turn left
			face = ( face - 1 ) % 4;
			car_turn_r();
			usleep(MOVE_SLEEP);
		}
		else if(ult_data[0] < DANGEROUS_DIS){
			//only right
			face = ( face + 1 ) % 4;
			car_turn_l();
			usleep(MOVE_SLEEP);
		}
		else if(ult_data[1] < DANGEROUS_DIS){
			//only front
			car_back();
			face = ( face - 1 ) % 4;
			car_turn_r();
			usleep(MOVE_SLEEP);
		}
		else if(ult_data[2] < DANGEROUS_DIS){
			//only left
			face = ( face - 1 ) % 4;
			car_turn_r();
			usleep(MOVE_SLEEP);
		}
		else if(ult_data[0] > MAX_DIS || ult_data[1] > MAX_DIS
				   || ult_data[2] > MAX_DIS){
			switch (face){
				case 0:
					car_x -= 2;
					break;
				case 1:
					car_y -= 2;
					break;
				case 2:
					car_x += 2;
					break;
				case 3:
					car_y += 2;
					break;
			}
			car_back();
			usleep(2*TURN_SLEEP);
		}
		else{
			//move forward
			switch (face){
				case 0:
					car_x ++;
					break;
				case 1:
					car_y ++;
					break;
				case 2:
					car_x --;
					break;
				case 3:
					car_y --;
					break;
			}
			map[car_x%50][car_y%50] = 0;
			car_forward();
			usleep(TURN_SLEEP);
		}
		printf("%d\n",sum);

		if(sum == 2500){
			car_stop();
			break;
		}
	}
}

int main()
{
	/*********init*******/
	zrcar_wheel_init();
	zrcar_ultra_init();
	zrcar_bluetooth_init();

	int old_map[50][50];
	int map[50][50];
	make_map(map,old_map);


	return 0;
}
